import math
import threading

import cv2

from HUD.HUD_deal import HUD


class HUD_control:
    def __init__(self, q_HUD_img_in, q_HUD_msg_in, q_HUD_img_out):
        self.q_HUD_img_in, self.q_HUD_msg_in, self.q_HUD_img_out = q_HUD_img_in, q_HUD_msg_in, q_HUD_img_out
        self.HUD_deal = HUD()
        self.HUD_deal.setBt(70)
        self.HUD_deal.setSpeed(60)
        self.img_thread = threading.Thread(target=self.img_update)
        self.msg_thread = threading.Thread(target=self.msg_update)

    def start_show(self):
        show_thread = threading.Thread(target=self.show)
        show_thread.start()

    def show(self):
        while True:
            if self.HUD_deal.d_img is not None:
                cv2.imshow('HUD', self.HUD_deal.d_img)
                cv2.waitKey(1)

    def start(self):
        self.msg_thread.start()
        self.img_thread.start()

    def msg_update(self):
        while True:
            msg = self.q_HUD_msg_in.get()
            if msg is not None:
                self.HUD_deal.setAng(int(msg[10]))
                self.HUD_deal.setRoll(msg[9])
                self.HUD_deal.setPitch(msg[8])
                self.HUD_deal.setAltitude(msg[16])
                self.HUD_deal.setSpeed(int(
                    math.sqrt(msg[11] * msg[11] + msg[12] * msg[12] + msg[13] * msg[13])))

    def img_update(self):
        while True:
            self.HUD_deal.set_img(self.q_HUD_img_in.get())
            self.HUD_deal.add_attitude()
            self.q_HUD_img_out.put(self.HUD_deal.get_output())
